#!/usr/bin/env python
#################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#################################################################################

# Authors: Gilbert #

import rospy
import numpy as np
import math
from math import pi
from geometry_msgs.msg import Twist, Point, Pose
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from std_srvs.srv import Empty
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from respawnGoal import Respawn  # 目标重生模块


class Env():
    def __init__(self):
        self.goal_x = 0
        self.goal_y = 0
        self.heading = 0  # 通过里程计回调函数赋值,为目标相对与机器人的角度
        self.initGoal = True
        self.get_goalbox = False
        self.position = Pose()
        # 发布速度信息
        self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        # 订阅里程计信息
        self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
        self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)  # 定义客户端
        self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
        self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
        self.respawn_goal = Respawn()
        self.past_distance = 0.

    def getGoalDistace(self):
        '''
        函数计算目标点到小车的距离(欧几里的距离)
        :return: 距离值
            round(x,[,n])函数返回浮点数x的四舍五入值
                x -- 数值表达式。
                n -- 数值表达式，表示从小数点位数。

            hypot(x,y)返回欧几里德范数 sqrt(x*x + y*y)
        '''
        goal_distance = round(math.hypot(self.goal_x - self.position.x, self.goal_y - self.position.y), 2)
        self.past_distance = goal_distance

        return goal_distance

    def getOdometry(self, odom):
        '''
        对于里程计来说关键是他的父级坐标系\子级坐标系以及位姿信息
        :param odom:
        :return:
        '''
        # 里程计位姿
        self.position = odom.pose.pose.position  # 机器人的x,y,z坐标
        orientation = odom.pose.pose.orientation  # 机器人的四元素数据
        orientation_list = [orientation.x, orientation.y, orientation.z, orientation.w]
        _, _, yaw = euler_from_quaternion(orientation_list)  # 从四元素转换到欧拉角,只需要考虑偏航角yaw

        goal_angle = math.atan2(self.goal_y - self.position.y, self.goal_x - self.position.x)  # 目标相对于机器人的角度

        heading = goal_angle - yaw  # 两者的角度差
        if heading > pi:
            heading -= 2 * pi

        elif heading < -pi:
            heading += 2 * pi

        self.heading = round(heading, 3)

    def getState(self, scan, past_action):
        # 雷达扫描数组
        scan_range = []
        # 目标相对于机器人的角度差
        heading = self.heading
        min_range = 0.20
        done = False

        for i in range(len(scan.ranges)):
            if scan.ranges[i] == float('Inf'):  # Inf为正无穷
                scan_range.append(3.5)  # 雷达扫描的最大距离为3.5米
            elif np.isnan(scan.ranges[i]):  # nan 表示不是一个数
                scan_range.append(0)  # 雷达扫描的最小距离为0.12
            else:
                scan_range.append(scan.ranges[i])

        if min_range > min(scan_range) > 0:
            done = True

        for pa in past_action:  # 将上一次的速度值加入列表中
            scan_range.append(pa)
        # 当前目标距离机器人的距离
        current_distance = round(math.hypot(self.goal_x - self.position.x, self.goal_y - self.position.y), 2)
        if current_distance < 0.2:  #
            self.get_goalbox = True

        return scan_range + [heading, current_distance], done

    def setReward(self, state, done):
        current_distance = state[-1]
        heading = state[-2]
        # print('cur:', current_distance, self.past_distance)

        distance_rate = (self.past_distance - current_distance)  # 上一次的距离与这一次的距离的差值
        # if distance_rate > 0:
        #    reward = 500.*distance_rate
        # if distance_rate == 0:
        #    reward = -10.
        # if distance_rate <= 0:
        #    reward = -15.
        # angle_reward = math.pi - abs(heading)
        # print('d', 500*distance_rate)
        reward = 500. * distance_rate  # + 3.*angle_reward 如果机器人与目标的距离越来越短,则获得正奖励,否则获得负奖励
        self.past_distance = current_distance

        if done:
            rospy.loginfo("Collision!!")
            reward = -75.
            self.pub_cmd_vel.publish(Twist())

        if self.get_goalbox:
            rospy.loginfo("Goal!!")
            reward = 100.
            self.pub_cmd_vel.publish(Twist())
            self.goal_x, self.goal_y = self.respawn_goal.getPosition(True, delete=True)
            self.goal_distance = self.getGoalDistace()
            self.get_goalbox = False

        return reward

    def step(self, action, past_action):
        linear_vel = action[0]
        ang_vel = action[1]

        vel_cmd = Twist()
        vel_cmd.linear.x = linear_vel
        vel_cmd.angular.z = ang_vel
        # 发布速度信息
        self.pub_cmd_vel.publish(vel_cmd)

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        state, done = self.getState(data, past_action)
        reward = self.setReward(state, done)

        return np.asarray(state), reward, done

    def reset(self):
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()  # 重置仿真环境
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)  # 接收话题
            except:
                pass

        if self.initGoal:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition()
            self.initGoal = False
        else:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition(True, delete=True)

        self.goal_distance = self.getGoalDistace()
        state, done = self.getState(data, [0., 0.])

        return np.asarray(state)
